/*
* Copyright (c) 2025 Shenzhen Kaihong Digital Industry Development Co., Ltd.
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
const { XTools } = require("../../engine/XTools");
const { LogManager } = require("../ros1/LogManager");
const { SelectRobotPart } = require("./SelectRobotPart");

class URDF {
    static rad(angle) {//角度转弧度
        return angle * Math.PI / 180;
    }
    static deg(rad) {//弧度转角度
        return rad * 180 / Math.PI;
    }
    static exportRPY(rpy) {
        return this.rad(rpy.x) + " " + this.rad(rpy.y) + " " + this.rad(rpy.z);
    }
    static exportInertial(part) {
        let customPart = SelectRobotPart.getInstance().searchCustomPart(part.token);
        return `\
        <inertial>
            <origin xyz="${customPart.xyz.x} ${customPart.xyz.y} ${customPart.xyz.z}" rpy="${this.exportRPY(customPart.rpy)}" />
            <mass value="${customPart.mass}" />
            <inertia ixx="0.1" ixy="0.0" ixz="0.0" iyy="0.1" iyz="0.0" izz="0.1" />
        </inertial>`;
    }
    static exportVisual(part) {
        let customPart = SelectRobotPart.getInstance().searchCustomPart(part.token);
        let visual = `\
        <visual>
            <origin xyz="${customPart.xyz.x} ${customPart.xyz.y} ${customPart.xyz.z}" rpy="${this.exportRPY(customPart.rpy)}" />
            <geometry>
`;
        if (customPart.type === "box") {
            visual += `\
                <box size="${customPart.box.x} ${customPart.box.y} ${customPart.box.z}" />
`;
        } else if (customPart.type === "cylinder") {
            visual += `\
                <cylinder length="${customPart.cylinder.height}" radius="${customPart.cylinder.radius}" />
`;
        } else if (customPart.type === "sphere") {
            visual += `\
                <sphere radius="${customPart.sphere.radius}" />
`;
        } else if (customPart.type === "mesh") {
            visual += `\
                <mesh filename="${customPart.mesh.path}" scale="${customPart.mesh.scale.x} ${customPart.mesh.scale.y} ${customPart.mesh.scale.z}" />
`;
        }
        visual += `\
            </geometry>
`;
        if (customPart.material == "color") {
            visual += `\
            <material name="color">
                <color rgba="${customPart.color.r} ${customPart.color.g} ${customPart.color.b} ${customPart.color.a}" />
            </material>
`;
        }
        visual += `
        </visual>`;
        return visual;
    }
    static exportCollision(part) {
        let customPart = SelectRobotPart.getInstance().searchCustomPart(part.token);
        let collision = `\
        <collision>
            <origin xyz="${customPart.xyz.x} ${customPart.xyz.y} ${customPart.xyz.z}" rpy="${this.exportRPY(customPart.rpy)}" />
            <geometry>
`;
        if (customPart.type === "box") {
            collision += `\
                <box size="${customPart.box.x} ${customPart.box.y} ${customPart.box.z}" />
`;
        } else if (customPart.type === "cylinder") {
            collision += `\
                <cylinder length="${customPart.cylinder.height}" radius="${customPart.cylinder.radius}" />
`;
        } else if (customPart.type === "sphere") {
            collision += `\
                <sphere radius="${customPart.sphere.radius}" />
`;
        } else if (customPart.type === "mesh") {
            collision += `\
                <mesh filename="${customPart.mesh.path}" scale="${customPart.mesh.scale.x} ${customPart.mesh.scale.y} ${customPart.mesh.scale.z}" />
`;
        }
        collision += `\
            </geometry>
        </collision>`;
        return collision;
    }
    static exportLink(part) {
        return `\
    <link name="${part.name}">
${this.exportInertial(part)}
${this.exportVisual(part)}
${this.exportCollision(part)}
    </link>
`;
    }
    static exportJoint(part, parent) {
        let joint = `\
    <joint name="joint_${part.name}" type="${part.jointType}">
        <parent link="${parent.name}" />
        <child link="${part.name}" />
        <origin xyz="${part.xyz.x} ${part.xyz.y} ${part.xyz.z}" rpy="${this.exportRPY(part.rpy)}" />
`;
        if (["revolute", "continuous", "prismatic", "planar"].includes(part.jointType)) {
            joint += `\
        <axis xyz="${part.axis.x} ${part.axis.y} ${part.axis.z}" />
`;
        }
        if (["revolute", "prismatic"].includes(part.jointType)) {//limits
            joint += `\
        <limit lower="${part.limit.lower}" upper="${part.limit.upper}" velocity="${part.limit.velocity}" effort="${part.limit.effort}" />
`;
        }
        if (["continuous", "prismatic"].includes(part.jointType)) {//dynamics
        }
        joint += `\
    </joint>
`;
        return joint;
    }
    static collectParts(parentName, parts, out) {
        for (let part of parts) {
            if (part.parent === parentName) {
                out.push(part);
                this.collectParts(part.name, parts, out);
            }
        }
    }
    static exportRobot(robot) {
        let xml = `<robot name = "${robot.name}">\n`;

        for (let part of robot.parts) {
            if (!part.isRoot) {
                continue;
            }
            let out = [part];
            this.collectParts(part.name, robot.parts, out);
            for (let p of out) {
                xml += this.exportLink(p);
                if (!p.isRoot) {
                    for (let parent of robot.parts) {
                        if (p.parent === parent.name) {
                            xml += this.exportJoint(p, parent);
                            break;
                        }
                    }
                }
            }
        }

        xml += `</robot>`;

        LogManager.I(xml);
        XTools.DownloadStringAsFile(robot.name + ".urdf", xml);
    }
}

module.exports = {
    URDF
};